﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Net;
using System.Net.Sockets;
using System.Threading;


    class KinectNetworkManager
    {
        private TcpListener kinectMachineListener;
        private Thread kinectListenThread;
        private List<KinectMachineThread> machines;
        private String currentJointInfo = "";
        private int machineID = 0;
        private KinectMachineThread currentTrackingMachine;
        private float currentRotation;
        private float oldMachineLimit = 50; // upper limit of angle difference before old machine should be changed
        private float newMachineLimit = 30; // upper limit of angle difference before new machine can be considered


        public KinectNetworkManager()
        {

            Console.WriteLine("Kinect machine connections");
            machines = new List<KinectMachineThread>();
            kinectMachineListener = new TcpListener(IPAddress.Any, 1241);
            kinectListenThread = new Thread(new ThreadStart(createServerForKinectMachines));
            kinectListenThread.Start();
            kinectListenThread.IsBackground = true;
        }

        private void createServerForKinectMachines()
        {

            kinectMachineListener.Start();

            while (true)
            {
                Console.WriteLine("Waiting for threads");
                TcpClient client = this.kinectMachineListener.AcceptTcpClient();
                KinectMachineThread kmt = new KinectMachineThread(this, client, machineID);
                machines.Add(kmt);
                machineID++;
                Console.WriteLine("Count is " + machines.Count);
                Console.WriteLine("New Kinect Machine Connected");
                if (machines.Count > 1)
                {
                    this.sortMachines();
                }
                Thread.Sleep(100);
            }

        }

        public String calculateJointMessage()
        {

            if (machines.Count > 0)
            {

                String message;
                float rotation;
                float a = currentRotation;
                
                currentTrackingMachine = this.setTrackingMachine();
                rotation = currentTrackingMachine.getRotationAngle();

                message = this.modifyRotationInformation(currentTrackingMachine);
                currentRotation = rotation + currentTrackingMachine.getAngularPosition();

                if (machines.Count() >= 2)
                {
                    message = this.modifyInferredJoints(message, rotation);
                }

                message = message.Replace("*", "");
                message = message.Replace("&", "");
                currentJointInfo = message;
            }
            else
            {
                currentJointInfo = "";
            }

            return currentJointInfo;
        }

        public void removeMachineClient(int removeID)
        {
            Console.WriteLine("id = " + removeID);

            for (int i = 0; i < machines.Count; i++)
            {
                KinectMachineThread kmt = machines[i];
                if (kmt.getID() == removeID)
                {
                    machines.Remove(kmt);
                    Console.WriteLine("machine ID " + removeID + " removed");
                    break;
                }
            }
        }



        public String modifyRotationInformation(KinectMachineThread kmt)
        {

            String s = kmt.getJointPositionString();

            if (s.Contains("ROTA:"))
            {
                try
                {
                    int start = s.IndexOf("ROTA:") + 5;
                    int end = s.IndexOf("!");
                    String rotString = s.Substring(start, end - start);
                    float newRotation = float.Parse(rotString) + kmt.getAngularPosition();
                    String newString = s.Replace(rotString, newRotation.ToString());
                    return newString;
                }
                catch {
                    return s;
                }
            }
            else
            {
                return s;
            }
        }

        //Sorts machines in order of angular position
        private void sortMachines()
        {
            machines.Sort(delegate(KinectMachineThread kmt1, KinectMachineThread kmt2) { return kmt1.getAngularPosition().CompareTo(kmt2.getAngularPosition()); });
        }

        private KinectMachineThread setTrackingMachine()
        {

            if (machines.Count() == 1)
            {
                return machines[0];
            }
            else
            {
                float x = currentTrackingMachine.getRotationAngle();
                float a = x + currentTrackingMachine.getAngularPosition();

                // Console.WriteLine("difference is " + Math.Abs(a - currentRotation));

                //if (Math.Abs(a - this.currentRotation) < 10)
                //{ //current tracking machine has similar rotation
                    if (Math.Abs(x) > newMachineLimit)
                    { // current tracking machine could be replaced

                        KinectMachineThread candidateMachine = this.getNextMachine(currentTrackingMachine, x);
                       // only change if next machine is better

                        if (!candidateMachine.Equals(currentTrackingMachine))
                        {

                            Console.WriteLine("Current rotation: " + currentRotation);
                            return this.compareMachines(currentTrackingMachine, candidateMachine);
                        }

                 //       Console.WriteLine(this.IsRotationInferred(currentTrackingMachine.getJointPositionString(), "ROTA"));
                    }
                    return currentTrackingMachine;
                //}
                //else
                //{ //current tracking machine has been corrupted - find next best machine

                //    // Console.WriteLine(currentTrackingMachine.getID() + " " + Math.Abs(a - currentRotation) + " bad");
                //    KinectMachineThread candidateMachine = this.getNextMachine(currentTrackingMachine, x);
                //    if (!candidateMachine.Equals(currentTrackingMachine))
                //    {
                //        return this.compareMachines(currentTrackingMachine, candidateMachine);
                //    }
                //    return currentTrackingMachine;
                //}
            }
        }

        //returns next machine next to current according to current machine rotation
        private KinectMachineThread getNextMachine(KinectMachineThread kmt, float dir)
        {

            KinectMachineThread nextKMT;
            float distToNext =  1000;
            int currentIndex = -1;

            for (int i = 0; i < machines.Count; i++)
            {
                if (machines[i].getID() == kmt.getID())
                {
                    currentIndex = i;
                    break;
                }
            }

            if (dir < 0)
            {//go clockwise (previous in line) 
                if (currentIndex == 0)
                {
                    nextKMT = machines[machines.Count - 1];
                }
                else
                {
                    nextKMT = machines[currentIndex - 1];
                }

                if (nextKMT.getAngularPosition() < kmt.getAngularPosition())
                {
                    distToNext = kmt.getAngularPosition() - nextKMT.getAngularPosition();
                }
                else
                {
                    distToNext = kmt.getAngularPosition() + 360 - nextKMT.getAngularPosition();
                }

            }
            else
            { //go anticlockwise (next in line)
                if (currentIndex == machines.Count - 1)
                {
                    nextKMT = machines[0];
                }
                else
                {
                    nextKMT = machines[currentIndex + 1];
                }

                if (nextKMT.getAngularPosition() > kmt.getAngularPosition())
                {
                    distToNext = nextKMT.getAngularPosition() - kmt.getAngularPosition();
                }
                else
                {
                    distToNext = nextKMT.getAngularPosition() + 360 - kmt.getAngularPosition();
                }

            }

            if (distToNext >= 120) {
                return kmt;
            }
            else{
                return nextKMT;
            }

        }

        private KinectMachineThread compareMachines(KinectMachineThread current, KinectMachineThread candidate)
        {
             
            float curRot = current.getRotationAngle();
            float candRot = candidate.getRotationAngle();

            if (curRot * candRot > 1)
            {
                Console.WriteLine(current.getID() + " " + curRot + " " + candidate.getID() + candRot + "  strange");
            }

            
            ////change if rotation of next machine is tracked and current is only inferred
            if (this.IsRotationInferred(current.getJointPositionString(), "ROTA") && !this.IsRotationInferred(candidate.getJointPositionString(), "ROTA"))
            {

                //change if the current rotation is outside the parameters
                if (Math.Abs(curRot) > 60 && Math.Abs(candRot) < 60)
                {
                    //current.resetAngularPosition();
                    //candidate.resetAngularPosition();
                    Console.WriteLine("candidate " + candidate.getID() + " is better: change");
                    Console.WriteLine(Math.Abs(currentRotation - (candidate.getAngularPosition() + candRot)));
                    Console.WriteLine();
                    return candidate;
                }
                else if (Math.Abs(candRot) > 60 && Math.Abs(curRot) < 60)
                {
        //            Console.WriteLine( "current " + current.getID() + " is better: dont change");
                    //current.resetAngularPosition();
                    //candidate.resetAngularPosition();
                    return current;
                }

                Console.WriteLine(curRot + " " + candRot + " rotation inference change to " + candidate.getID());
                this.setNewAngularPosition(curRot, candRot, current, candidate);
                Console.WriteLine(Math.Abs(currentRotation - (candidate.getAngularPosition() + candRot)));
                Console.WriteLine();
                return candidate;

            }

            else if (!this.IsRotationInferred(current.getJointPositionString(), "ROTA") && !this.IsRotationInferred(candidate.getJointPositionString(), "ROTA"))
            { 
                Console.WriteLine("both non-inferred");

                if (Math.Abs(curRot) > Math.Abs(candRot))
                {
                    Console.WriteLine("angle of candidate " + candidate.getID() + " is smaller, changed");
                    Console.WriteLine(Math.Abs(currentRotation - (candidate.getAngularPosition() + candRot)));
                    Console.WriteLine();
                    return candidate;
                }
                else
                {
              //      Console.WriteLine("angle of current " + candidate.getID() + " is smaller, not changed");
                    return current;
                }
            
            }

            else if (this.IsRotationInferred(current.getJointPositionString(), "ROTA") && this.IsRotationInferred(candidate.getJointPositionString(), "ROTA"))
            {
          //      Console.WriteLine("both inferred, returning current " + current.getID());
                return current;
            }

            else
            {
          //      Console.WriteLine("normal current " + current.getID() + " candidate " + candidate.getID());
                return current;
            }

        }

        private void setNewAngularPosition(float oldRot, float newRot, KinectMachineThread oldKMT, KinectMachineThread newKMT) {

            float oldRotAngle = oldRot;
            float newRotAngle = newRot;
            int oldPos = oldKMT.getAngularPosition();

            int x = Convert.ToInt32(oldPos + oldRotAngle + (newRotAngle * -1));

            newKMT.setAngularPosition(x);
            oldKMT.resetAngularPosition();

         //   Console.WriteLine("old pos = " + oldPos + " old rot = " + oldRotAngle + " new rot = " + newRotAngle + " x = " + x + " new angular pos = " + newKMT.getAngularPosition());

        //    Console.WriteLine((oldPos + oldRotAngle) + " " + (newKMT.getAngularPosition() + newRotAngle));
         //   Console.WriteLine("changed AP " + newKmt.getID() + " to " + newKmt.getAngularPosition() + "," +  oldKmt.getID() +  " is " + oldKmt.getAngularPosition());

        }

        private bool IsRotationInferred(String s, String header)
        {
            if (s.Contains(header)) {
                int index = s.IndexOf(header);
                if (index == 0)
                {
                    return false;
                }
                else
                {
                    if (s[index - 1].ToString().Equals("&") || s[index - 1].ToString().Equals("*")) {
                        return true;
                    }
                } 
            }
            
            return false;
        }

        private String modifyInferredJoints(String s, float rot)
        {

            List<String> currentUntracked = getInferredBones(s, "*");
            List<String> currentInferred = getInferredBones(s, "&");

        //    Console.WriteLine(currentTrackingMachine.getID());

            if (currentInferred.Count() > 0)
            {

                KinectMachineThread newMachine = this.getNextMachine(currentTrackingMachine, rot);
           //     Console.WriteLine(currentTrackingMachine.getID() + " " + currentJointInfo);
                if (!newMachine.Equals(currentTrackingMachine))
                {

                        String newMachineInfo = newMachine.getJointPositionString();

                          if (newMachineInfo.Length > 10)
                          {
                              List<String> newUntracked = getInferredBones(newMachineInfo, "*");
                              List<String> newInferred = getInferredBones(newMachineInfo, "&");

                              double angleDiff = ((newMachine.getAngularPosition() - currentTrackingMachine.getAngularPosition() - rot) * (Math.PI / 180.0));

                              String upperArmLeftString = "ELSL";
                              String lowerArmLeftString = "WLEL";
                              String upperArmRightString = "ERSR";
                              String lowerArmRightString = "WRER";

                              bool isChanged = false;

                              String modifiedMessage = s;

                              //int currentLeftArmValue = 0;
                              //int candidateLeftArmValue = 0;


                              //if (currentUntracked.Contains(upperArmLeftString)) {
                              //    currentLeftArmValue -= 2;                              
                              //}
                              //else if (currentInferred.Contains(upperArmLeftString)) {
                              //    currentLeftArmValue--;
                              //}

                              //if (currentUntracked.Contains(lowerArmLeftString))
                              //{
                              //    currentLeftArmValue -= 2;
                              //}
                              //else if (currentInferred.Contains(lowerArmLeftString))
                              //{
                              //    currentLeftArmValue--;
                              //}

                              //if (newUntracked.Contains(upperArmLeftString))
                              //{
                              //    candidateLeftArmValue -= 2;
                              //}
                              //else if (newInferred.Contains(upperArmLeftString))
                              //{
                              //    candidateLeftArmValue--;                              }

                              //if (newUntracked.Contains(lowerArmLeftString))
                              //{
                              //    candidateLeftArmValue -= 2;
                              //}
                              //else if (newInferred.Contains(lowerArmLeftString))
                              //{
                              //    candidateLeftArmValue--;
                              //}

                              //if (candidateLeftArmValue > currentLeftArmValue) {
                              //      currentJointInfo = this.createInferredJoints(currentJointInfo, newMachineInfo, upperArmLeftString, angleDiff);
                              //      currentJointInfo = this.createInferredJoints(currentJointInfo, newMachineInfo, lowerArmLeftString, angleDiff);
                              //      Console.WriteLine("change arm");
                              //}

                              if (currentUntracked.Contains(upperArmLeftString))
                              {
                                  if (!newUntracked.Contains(upperArmLeftString) && !newInferred.Contains(upperArmLeftString))
                                  {
                                      modifiedMessage = this.createInferredJoints(modifiedMessage, newMachineInfo, upperArmLeftString, angleDiff);
                                      isChanged = true;
                                      //                Console.WriteLine("change untracked");
                                  }
                              }
                              else if (currentInferred.Contains(upperArmLeftString))
                              {
                                  if (!newUntracked.Contains(upperArmLeftString))
                                  {
                                      modifiedMessage = this.createInferredJoints(modifiedMessage, newMachineInfo, upperArmLeftString, angleDiff);
                                      isChanged = true;
                         //             Console.WriteLine("change inferred leftshoulder ");
                                  }
                              }

                              if (isChanged)
                              {
                                  modifiedMessage = this.createInferredJoints(modifiedMessage, newMachineInfo, lowerArmLeftString, angleDiff);
                              }

                              isChanged = false;

                              if (currentUntracked.Contains(upperArmRightString))
                              {
                                  if (!newUntracked.Contains(upperArmRightString) && !newInferred.Contains(upperArmRightString))
                                  {
                                      modifiedMessage = this.createInferredJoints(modifiedMessage, newMachineInfo, upperArmRightString, angleDiff);
                                      isChanged = true;
                                      //           Console.WriteLine("change untracked");
                                  }
                              }
                              else if (currentInferred.Contains(upperArmRightString))
                              {
                                  if (!newUntracked.Contains(upperArmRightString))
                                  {
                                      modifiedMessage = this.createInferredJoints(modifiedMessage, newMachineInfo, upperArmRightString, angleDiff);
                                      isChanged = true;
                                      //           Console.WriteLine("change inferred right shoulder");
                                  }
                              }

                              if (isChanged)
                              {
                                  modifiedMessage = this.createInferredJoints(modifiedMessage, newMachineInfo, lowerArmRightString, angleDiff);
                              }

                              return modifiedMessage;

                          }
                          return s;
                }
                return s;

            }
            return s;

        }

        private List<String> getInferredBones(String inputString, String identifier) {

            List<String> s = new List<String>();

            for (int e = 0; e < inputString.Length; e++)
            {

                if (inputString[e].ToString().Equals(identifier))
                {
                    int endIndex = inputString.IndexOf(":", e) - 1;
                    String jointHeader = inputString.Substring(e + 1, endIndex - e);
                    s.Add(jointHeader);
                }
            }

            return s;       
        }

        private String createInferredJoints(String currentMessage, String inferredMessage, String header, double r) {

     //       Console.WriteLine("old " + currentMessage);
          //  Console.WriteLine(header + " " + currentMessage.IndexOf(header));
            String stringToReplace;
            String replacementString;
            String rotatedJointMessage;
            String newMessage;
            float f = (float)r;
          //  Console.WriteLine("cur "+ currentMessage);
         //   Console.WriteLine("inf " + inferredMessage);
            try
            {
                int i = currentMessage.IndexOf(header);
                int start = currentMessage.IndexOf(":", i) + 1;
                int end = currentMessage.IndexOf("!", start);
                stringToReplace = currentMessage.Substring(start, end - start);
         //          Console.WriteLine(stringToReplace);

                start = inferredMessage.IndexOf(header);
                end = inferredMessage.IndexOf("!", start);
                replacementString = inferredMessage.Substring(start, end - start);
                rotatedJointMessage = rotateJointMessage(replacementString, f);
                newMessage = currentMessage.Replace(stringToReplace, rotatedJointMessage);
   //             Console.WriteLine("new " + newMessage);
                return newMessage;
            }
            catch {
                return currentMessage;
            }
            
        }

        private String rotateJointMessage(String s, float f) {

            double xVal, yVal, zVal;
            String xString, yString, zString;

            int start = s.IndexOf(":") + 2;
            int end = s.IndexOf("Y", start);
            xString = s.Substring(start, end - start);

            start = end + 1;
            end = s.IndexOf("Z", start);
            yString = s.Substring(start, end - start);

            start = end + 1;
            end = s.Length;
            zString = s.Substring(start, end - start);

            xVal = float.Parse(xString);
            yVal = float.Parse(yString);
            zVal = float.Parse(zString);


            float[] YAxis = { 0, 1, 0 };
            double[] vector = { xVal, yVal, zVal};

            String newVectorString = rotateVector(YAxis, vector, f);
            String bob = s.Replace(s.Substring(newVectorString.IndexOf(":") + 1), newVectorString);

            return newVectorString;
        
        }

        private String rotateVector(float[] axis, double[] vector, float angle) {

            double d = 1 - Math.Cos(angle);
            double X = axis[0];
            double Y = axis[1];
            double Z = axis[2]; 
            double a1, a2, a3, b1, b2, b3, c1, c2, c3;
            double newX, newY, newZ;

           
            a1 = (d * X * X) + Math.Cos(angle);
            a2 = (d * X * Y) - (Math.Sin(angle) * Z);
            a3 = (d * X * Z) + (Math.Sin(angle) * Y);
            b1 = (d * X * Y) + (Math.Sin(angle) * Z);
            b2 = (d * Y * Y) + Math.Cos(angle);
            b3 = (d * Y * Z) - (Math.Sin(angle) * X);
            c1 = (d * X * Z) - (Math.Sin(angle) * Y);
            c2 = (d * Y * Z) + (Math.Sin(angle) * X);
            c3 = (d * Z * Z) + Math.Cos(angle);

            newX = Math.Round((a1 * vector[0]) + (a2 * vector[1]) + (a3 * vector[2]), 4);
            newY = Math.Round((b1 * vector[0]) + (b2 * vector[1]) + (b3 * vector[2]), 4);
            newZ = Math.Round((c1 * vector[0]) + (c2 * vector[1]) + (c3 * vector[2]), 4);

            String newVectorString = "X" + newX + "Y" + newY + "Z" + newZ;
            return newVectorString;

        }







    }

